clc
clear
close all
load('data.mat');
global d2r Td hInt

t2 = ITCG1(:,1);
x2 = ITCG1(:,2);
y2 = ITCG1(:,3);
z2 = ITCG1(:,4);
V2 = ITCG1(:,5);
fpa2 = ITCG1(:,6)/d2r;
azi2 = ITCG1(:,7)/d2r;
lambda2 = ITCG1(:,8)/d2r;
phi2 = ITCG1(:,9)/d2r;
tPNG2 = ITCG1(:,10);
alpha2 = ITCG1(:,11)/d2r;
bank2 = ITCG1(:,12)/d2r;
et2 = ITCG1(:,13);
r2 = ITCG1(:,14);
dr2 = ITCG1(:,15);
delta2 = ITCG1(:,16)/d2r;
eta2 = ITCG1(:,17)/d2r;
perror2 = sqrt(x2(end)^2 + y2(end)^2 + z2(end)^2);

t3 = ITCG2(:,1);
x3 = ITCG2(:,2);
y3 = ITCG2(:,3);
z3 = ITCG2(:,4);
V3 = ITCG2(:,5);
fpa3 = ITCG2(:,6)/d2r;
azi3 = ITCG2(:,7)/d2r;
lambda3 = ITCG2(:,8)/d2r;
phi3 = ITCG2(:,9)/d2r;
tPNG3 = ITCG2(:,10);
alpha3 = ITCG2(:,11)/d2r;
bank3 = ITCG2(:,12)/d2r;
et3 = ITCG2(:,13);
r3 = ITCG2(:,14);
dr3 = ITCG2(:,15);
delta3 = ITCG2(:,16)/d2r;
eta3 = ITCG2(:,17)/d2r;
perror3 = sqrt(x3(end)^2 + y3(end)^2 + z3(end)^2);

t4 = ITCG3(:,1);
x4 = ITCG3(:,2);
y4 = ITCG3(:,3);
z4 = ITCG3(:,4);
V4 = ITCG3(:,5);
fpa4 = ITCG3(:,6)/d2r;
azi4 = ITCG3(:,7)/d2r;
lambda4 = ITCG3(:,8)/d2r;
phi4 = ITCG3(:,9)/d2r;
tPNG4 = ITCG3(:,10);
alpha4 = ITCG3(:,11)/d2r;
bank4 = ITCG3(:,12)/d2r;
et4 = ITCG3(:,13);
r4 = ITCG3(:,14);
dr4 = ITCG3(:,15);
delta4 = ITCG3(:,16)/d2r;
eta4 = ITCG3(:,17)/d2r;
perror4 = sqrt(x4(end)^2 + y4(end)^2 + z4(end)^2);

f = figure('Name', 'Trajectory');
plot3(x2/1e3,y2/1e3,z2/1e3,'--','LineWidth',2);grid;hold on;
plot3(x3/1e3,y3/1e3,z3/1e3,':','LineWidth',2);
plot3(x4/1e3,y4/1e3,z4/1e3,'-.','LineWidth',2);
xlabel('x/km');ylabel('y/km');zlabel('z/km')
legend('飞行器1','飞行器2','飞行器3');legend('boxoff');
saveas(f, f.Name, 'svg')

% f = figure('Name', '飞行轨迹的铅锤投影');
% plot(x2/1e3,z2/1e3,'--', x3/1e3,z3/1e3,':', x4/1e3,z4/1e3,'-.','LineWidth',2);
% xlabel('x/km');ylabel('z/km');
% legend('ITCG1','ITCG2','ITCG3','Location','northwest');legend('boxoff');
% saveas(f, f.Name, 'svg')

f = figure('Name', '飞行轨迹的水平投影');
plot(x2/1e3,y2/1e3,'--',x3/1e3,y3/1e3,':',x4/1e3,y4/1e3,'-.','LineWidth',2);
xlabel('x/km');ylabel('y/km');
legend('飞行器1','飞行器2','飞行器3');legend('boxoff');
saveas(f, f.Name, 'svg')

f = figure('Name', 'speed');
plot(t2, V2,'--', t3, V3,':', t4, V4,'-.','LineWidth',2);
xlabel('时间（s）'); ylabel("速度（m/s）");
legend('飞行器1','飞行器2','飞行器3');legend('boxoff');
saveas(f, f.Name, 'svg')

f = figure('Name', 'flight path angle');
plot(t2,fpa2,'--',t3,fpa3,':',t4,fpa4,'-.','LineWidth',2);
xlabel('时间（s）'); ylabel("弹道倾角(deg)");
legend('飞行器1','飞行器2','飞行器3');legend('boxoff');
saveas(f, f.Name, 'svg')

f = figure('Name', 'azimuth angle');
plot(t2, azi2,'--', t3, azi3,':', t4, azi4,'-.','LineWidth',2);hold on;
xlabel('时间（s）'); ylabel("弹道偏角(deg)");
legend('飞行器1','飞行器2','飞行器3');legend('boxoff');
saveas(f, f.Name, 'svg')

f = figure('Name', 'guidance command');
subplot(2,1,1)
plot(t2,alpha2,'--',t3,alpha3,':', t4, alpha4,'-.','LineWidth',2);
xlabel('时间（s）'); ylabel("攻角（deg）");
legend('飞行器1','飞行器2','飞行器3');legend('boxoff');
subplot(2,1,2)
plot(t2,bank2,'--',t3,bank3,':', t4, bank4,'-.', 'LineWidth',2);
xlabel('时间（s）'); ylabel("倾侧角（deg）");
saveas(f, f.Name, 'svg')

f = figure('Name', 'tracking error');
plot(t2(1:Td/hInt), et2(1:Td/hInt),'--', t3(1:Td/hInt), et3(1:Td/hInt), ':', t4(1:Td/hInt), et4(1:Td/hInt),'-.','LineWidth',2);
xlabel('时间（s）'); ylabel("时间误差/s");
legend('飞行器1','飞行器2','飞行器3','Location','southeast');legend('boxoff');
saveas(f, f.Name, 'svg')